#include "Game Engine/Header/BulletUtil.h"

// DirectX to Bullet Helper Functions
btVector3 BTVector3( const D3DXVECTOR3& v )
{
	return btVector3( v.x, v.y, v.z );
}

btQuaternion BTQuaternion( const D3DXQUATERNION &q )
{
	return btQuaternion(q.x, q.y, q.z, q.w);
}

btTransform BTMatrixRotation( const D3DXMATRIX& m )
{
	btTransform trans;
	//trans.setOrigin( btVector3( m._41, m._42, m._43 ) );
	trans.setRotation( btQuaternion( m._11, m._22, m._33, 1.0f ) );

	return trans;
}

//Bullet-to-DX Helper Functions
D3DXVECTOR3 BT2DX_VECTOR3(const btVector3 &v)
{
	return D3DXVECTOR3(v.x(), v.y(), v.z());
}

D3DXQUATERNION BT2DX_QUATERNION(const btQuaternion &q)
{
	return D3DXQUATERNION(q.x(), q.y(), q.z(), q.w());
}

D3DXMATRIX BT2DX_MATRIX(const btMotionState &ms)
{
	btTransform world;
	ms.getWorldTransform(world);
	return BT2DX_MATRIX(world);
}

D3DXMATRIX BT2DX_MATRIX(const btTransform &ms)
{
	btQuaternion q = ms.getRotation();
	btVector3 p = ms.getOrigin();

	D3DXMATRIX pos, rot, world;
	D3DXMatrixTranslation(&pos, p.x(), p.y(), p.z());
	D3DXMatrixRotationQuaternion(&rot, &BT2DX_QUATERNION(q));
	D3DXMatrixMultiply(&world, &rot, &pos);
	
	return world;
}